#include "Interface_Motor.h"
#include "Interface_ROS.h"

const uint8_t _data_H[8] = {0x05, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0xFF};
const uint8_t _data_L[8] = {0x04, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0xFE};

const uint8_t angle_date[7] = {0x36, 0x01, 0x00, 0x01, 0x00, 0x00, 0x6B};

void ShowAnglePool()
{
	for (int j = 0; j < RawAnglePool.size; ++j)
	{
		printf("Motor ID: %d, Raw Angle Data: ", RawAnglePool.data[j].motorID);
		for (int i = 0; i < 7; ++i)
		{
			printf("0x%02X ", RawAnglePool.data[j].rawAngle[i]);
		}
		printf("\n");
	}
	printf("\n");
}

void ShowMotorPool()
{
	for (int j = 0; j < 6; ++j)
	{
		printf("ID:%d  ", MotorPool[j].id);
		printf("%f ", MotorPool[j].current_angle);
		printf("\n");
	}
	printf("\n");
}

int main()
{
	Pairpool_Init(&PairPool);


	if (ROS_Check(_data_H) == HAL_OK1)
	{
		BufferMov_ROS(_data_H, &PairPool);
	}

	if (ROS_Check(_data_L) == HAL_OK1)
	{
		BufferMov_ROS(_data_L, &PairPool);
	}

	ROS_Process(&PairPool, MotorPool);
	return 0;
}
